#include <Movement.h>
 
 int leftValue[2] = {0,0};
 int rightValue[2] = {0,0};
 int centerValue[2] = {0,0};
 int RM_EN = 10;
 int LM_EN = 11;
 int LFWD = 13;
 int LREV = 12;
 int RFWD = 8;
 int RREV = 9;
 Movement mover(LFWD,LREV,RFWD,RREV,LM_EN,RM_EN);

void setup() {
  pinMode(5, INPUT);
  pinMode(6, INPUT);
  pinMode(7, INPUT);
}

void loop() {
  leftValue[1] = leftValue[0];
  centerValue[1] = centerValue[0];
  rightValue[1] = rightValue[0];
  leftValue[0] = digitalRead(5);
  centerValue[0] = digitalRead(6);
  rightValue[0] = digitalRead(7);

  if((leftValue[0]==1) && (centerValue[0]==0) && (rightValue[0]==1))
    mover.setMove(50,75);
  if((leftValue[0]==1) && (centerValue[0]==1) && (rightValue[0]==1))
    mover.setMove(50,75);
  if((leftValue[0]==1) && (centerValue[0]==1) && (rightValue[0]==0))
    mover.setMove(85,75);
  if((leftValue[0]==1) && (centerValue[0]==0) && (rightValue[0]==0))
    mover.setMove(85,75);
  if((leftValue[0]==0) && (centerValue[0]==0) && (rightValue[0]==1))
    mover.setMove(15,75);
  if((leftValue[0]==0) && (centerValue[0]==1) && (rightValue[0]==1))
    mover.setMove(15,75);
  if((leftValue[0]==1) && (centerValue[0]==1) && (rightValue[0]==1))
  {
    if((leftValue[1]==1) && (centerValue[1]==1) && (rightValue[1]==0))
    {
      mover.setMove(100,50);
      while(digitalRead(6)){}
      mover.setMove(50,75);
    }
    if((leftValue[1]==0) && (centerValue[1]==1) && (rightValue[1]==1))
    {
      mover.setMove(0,50);
      while(digitalRead(6)){}
      mover.setMove(50,75);
    }
  }
  delay(200);
}
